/*
 * @Author: your name
 * @Date: 2021-06-12 07:28:47
 * @LastEditTime: 2021-06-18 21:56:27
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /lab_1_2_geometry_unfinished/lab_1_2.cpp
 */
#include "lab_1_2.h"

#include "dataset.h"
#include "local_coordinate_system.h"
#include "viewer_3d.h"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"

Lab_1_2::Lab_1_2(const std::string& data_path)
  : data_path_{data_path}
  , window_name_{"World point in camera"}
{}

void Lab_1_2::run()
{
  // Set up dataset.
  Dataset dataset{data_path_};

  // Define local coordinate system based on the position of a light pole.
  const GeodeticPosition light_pole_position{59.963516, 10.667307, 321.0};
  const LocalCoordinateSystem local_system(light_pole_position);

  // Construct viewers.
  cv::namedWindow(window_name_);
  Viewer3D viewer;

  // Process each image in the dataset.
  for (DataElement element{}; dataset >> element;)
  {
    // Compute the pose of the body in the local coordinate system.

    const Sophus::SE3d pose_local_body = local_system.toLocalPose(element.body_position_in_geo,
                                                                  element.body_attitude_in_geo.toSO3());

    // Add body coordinate axis to the 3D viewer.
    viewer.addBody(pose_local_body,element.img_num);
    // Compute the pose of the camera relative to the body.

    // Todo: Construct pose_body_camera correctly using element.        
    // changes from toSO3 to toquanternion()
    const Sophus::SE3d pose_body_camera{element.camera_attitude_in_body.toQuaternion(),element.camera_position_in_body.toVector()};

    // Compute the pose of the camera relative to the local coordinate system.
    // Todo: Construct pose_local_camera correctly using the poses above.
    const Sophus::SE3d pose_local_camera=pose_local_body*pose_body_camera;

    Eigen::Vector3d aaa1 = pose_local_camera.matrix3x4().block(0,3,1,3);
    Eigen::Vector3d aaa2 = pose_local_camera.matrix3x4().block(0,2,1,3);
    double Z = aaa1.dot(aaa2);
    std::cout<<"Z:"<<Z<<std::endl;
    // Construct a camera model based on the intrinsic calibration and camera pose.

    const PerspectiveCameraModel camera_model{element.intrinsics.toCalibrationMatrix(),
                                             pose_local_camera,
                                             element.intrinsics.toDistortionCoefficientVector()};

    // Undistort image. 

    std ::cout <<"size:"<<element.image.size()<<std::endl;
    cv::Mat undistorted_img=camera_model.undistortImage(element.image);
    //

            // cv::imshow("aaa1",element.image);

    // Project world point (the origin) into the image.
    
    const Eigen::Vector2d pix_pos = camera_model.projectWorldPoint(Eigen::Vector3d(0,0,0));

    // Draw a marker in the image at the projected position.
    const Eigen::Vector2i pix_pos_int = (pix_pos.array().round()).cast<int>();
    cv::drawMarker(undistorted_img,{pix_pos_int.x(),pix_pos_int.y()},{0.,0.,255.},cv::MARKER_CROSS, 40, 3);
//{ pix_pos_int.x() ,pix_pos_int.y() }
 std ::cout <<pix_pos.x()<<","<<pix_pos.y()<<std::endl;
    // Show the image.
    // Todo: Write line of code below to show the image with the marker.
    cv::imshow(window_name_,undistorted_img);

    // Add the camera to the 3D viewer.
    // Todo: Write line of code below to add body to viewer.
    viewer.addCamera(camera_model,element.image,element.img_num);
    // Update the windows.
    viewer.spinOnce();
    cv::waitKey(100);
  }

  // Remove image viewer.
  cv::destroyWindow(window_name_);

  // Run 3D viewer until stopped.
  viewer.spin();
}
